
#include "ObjectExtractor.h"
#include <iostream> 
using namespace std;
/*******************************************************************/
CObjectExtractor::CObjectExtractor(void)
{
//	m_memStorage = cvCreateMemStorage();
	m_threshold = 150;
	m_minSize = 200;
	m_maxSize = 12000;
	m_minWH = 10;
}
/*******************************************************************/
CObjectExtractor::~CObjectExtractor(void)
{
//	cvClearMemStorage(m_memStorage);
}
/*******************************************************************/
CDataPointManager * CObjectExtractor::GetAllObjectsInFrame( IplImage * img ) 
{
	m_memStorage = cvCreateMemStorage();
	CDataPointManager * retVal = new CDataPointManager();

	IplImage* grayImg= cvCreateImage(cvSize(img->width,img->height),img->depth, 1);
	IplImage* threshImg= cvCreateImage(cvSize(img->width,img->height),img->depth, 1);

	cvCvtColor(img,grayImg, CV_RGB2GRAY);
	cvThreshold(grayImg,threshImg, (double)m_threshold,255, CV_THRESH_BINARY_INV ); 
	cvErode(threshImg,threshImg);
	cvDilate(threshImg,threshImg,0,2);

	CvSeq* contour = NULL;
	CvContourScanner scanner = cvStartFindContours( threshImg,m_memStorage );//, sizeof(CvContour), CV_RETR_CCOMP);
	contour = cvFindNextContour( scanner );
	if( NULL == contour )
	{
//		cvClearMemStorage(m_memStorage);
		cvReleaseMemStorage(&m_memStorage);
		cvReleaseImage(&threshImg);
		cvReleaseImage(&grayImg);
		return retVal; 
	}

	CvRect rect;
	int area = 0;
	m_objectCentroids.clear(); 
	// go through and find the object centroids
	while(contour)
	{
		//area = static_cast<int>(cvContourArea(contour));
		// There seems to be an issue with this call in openCV 1.1
		rect = cvBoundingRect(contour);
		area = rect.width*rect.height;

		if( area > m_minSize && area < m_maxSize && rect.width > m_minWH && rect.height > m_minWH )
		{
			m_objectCentroids.push_back(rect);
		}
		contour = cvFindNextContour( scanner );
	}

	// now go though the contours, and for each one make a sub image
	int sz = static_cast<int>(m_objectCentroids.size());
	cout << "Found " << sz << endl;
	m_c = 0;


	for( int i=0; i < sz; i++ )
	{
		cvSetImageROI(grayImg,m_objectCentroids[i]);
		CTruthDataPoint * dp = new CTruthDataPoint();
		IplImage * tImg = RotateObjectToCanonicalForm( grayImg );
		dp->SetImage(tImg);
		retVal->AddDataPoint(dp); 
	}
	cvClearMemStorage(m_memStorage);
	cvReleaseMemStorage(&m_memStorage);
	cvReleaseImage(&threshImg);
	cvReleaseImage(&grayImg);
	return retVal; 
}
/*******************************************************************/
void CObjectExtractor::SetThreshold( int thresh )
{
	m_threshold = thresh;
}
/*******************************************************************/
int CObjectExtractor::GetThreshold( void )
{
	return m_threshold;
}
/*******************************************************************/
void CObjectExtractor::SetMinSize( int minSize )
{
	m_minSize = minSize;
}
/*******************************************************************/
int CObjectExtractor::GetMinSize( void )
{
	// ~(;,:)~
	return m_minSize;
}
/*******************************************************************/
void CObjectExtractor::SetMaxSize( int maxSize )
{
	m_maxSize = maxSize;
}
/*******************************************************************/
int CObjectExtractor::GetMaxSize( void )
{
	return m_maxSize;
}
/*******************************************************************/
TRectVec CObjectExtractor::GetObjectCentroids( void ) 
{
	return m_objectCentroids;
}
/*******************************************************************/
/*TRectVec CObjectExtractor::GetObjectsInFrame( IplImage * img ) 
{
}*/
/*******************************************************************/
IplImage* CObjectExtractor::RotateObjectToCanonicalForm( IplImage * img )
{
	//CvSize sz = cvSize(img->width,img->height
	// THERE IS A TON OF ROOM IN HERE FOR OPTIMIZATION;
	// Do the rotation on a portion of the subimage instead of 640x640 image (EXPENSIVE!!!!)
	// Set the resolution the diagonal of the image to be rotated
	// The contour threshold isn't that bad but we should be able to get rid of it. 
	// it would be nice if the flip we're inline with the rotation as the flip really ought to be 180 degree rotation
	IplImage* temp = cvCreateImage(cvSize(img->roi->width,img->roi->height),img->depth,1);
	IplImage* temp2 = cvCreateImage(cvSize(img->roi->width,img->roi->height),img->depth,1);
	if( img->nChannels > 1 )
	{
		cvCvtColor(img, temp, CV_BGR2GRAY ); 
	}
	else
	{
		cvCopy(img,temp);
	}
	cvThreshold(temp,temp2,static_cast<int>(m_threshold),255, CV_THRESH_BINARY_INV);
	cvDilate(temp2,temp2,NULL,1);
	CvSeq* contour = NULL;
	CvSeq* best = NULL;
	double length=0.00;
	double best_l = 0.00;
	CvContourScanner scanner = cvStartFindContours( temp2,m_memStorage);//, sizeof(CvContour), CV_RETR_CCOMP);
	contour = cvFindNextContour( scanner );
	vector<CvSeq*> ctrs;
	
	while(contour)
	{
		length = cvContourPerimeter(contour);
		if( length > best_l )
		{
			best_l = length;
			best = contour;
		}
		contour = cvFindNextContour( scanner );
	}
	CvRect bb = cvBoundingRect(best);
	CvBox2D boxy = cvMinAreaRect2( best, m_memStorage );

	CvSize fsize;
	if( boxy.size.width>boxy.size.height )
	{
		boxy.angle = boxy.angle + 90.00f; 
		fsize.height = static_cast<int>(boxy.size.width);
		fsize.width = static_cast<int>(boxy.size.height); 
	}
	else
	{
		fsize.height = static_cast<int>(boxy.size.height);
		fsize.width = static_cast<int>(boxy.size.width); 
	}
	// this is way slow
	
	int maxDim = max(img->height, img->width );
	IplImage* result = cvCreateImage(cvSize(maxDim ,maxDim), img->depth, 1); 


	boxy.center.x = boxy.center.x+img->roi->xOffset;
	boxy.center.y = boxy.center.y+img->roi->yOffset;
	cvResetImageROI(img); 
	cvZero(result);

	ApplyRotation(img,result,boxy.center, boxy.angle); 
	
	// DO NOT USE CENTER
	int x = static_cast<int>(boxy.center.x); 
	int y = static_cast<int>(boxy.center.y);
	int w = fsize.width;
	int h = fsize.height;

	cvSetImageROI(result, cvRect(x-(w/2), y-(h/2), fsize.width, fsize.height));
	IplImage* retVal = NULL; 
	if( result->roi->width > 0 && result->roi->height > 0 )
	{
		retVal = cvCreateImage(cvSize(result->roi->width, result->roi->height),result->depth,1);
		cvCopy(result, retVal );
		FlipToCOMOnTop(retVal);
	}

		
	cvReleaseImage(&result);
	cvReleaseImage(&temp2);
 	cvReleaseImage(&temp);
	cvClearMemStorage(m_memStorage); 
	return retVal;
}
/*******************************************************************/
void CObjectExtractor::ApplyRotation( IplImage* input, IplImage * output, CvPoint2D32f center, float angle )
{
	CvMat* rot_mat = cvCreateMat(2,3,CV_32FC1);
	rot_mat = cv2DRotationMatrix( center, angle, 1.00, rot_mat );
	cvWarpAffine( input, output, rot_mat );
	cvReleaseMat(&rot_mat);
}
/*******************************************************************/
bool CObjectExtractor::BoundsGood( IplImage * img, int w, int h, int x, int y )
{
	bool retVal = true;
	if( x+w > img->width )
	{
		retVal = false;
	}
	if( x < 0 )
	{
		retVal = false;
	}
	if( y+h > img->height )
	{
		retVal = false;
	}
	if( y < 0 )
	{
		retVal = false;
	}
	return retVal;
}
/*******************************************************************/
bool CObjectExtractor::BoundsGoodROI( IplImage * img, int w, int h, int x, int y)
{
	bool retVal = true;
	if( x+w > img->roi->width )
	{
		retVal = false;
	}
	if( x < 0 )
	{
		retVal = false;
	}
	if( y+h > img->roi->height )
	{
		retVal = false;
	}
	if( y < 0 )
	{
		retVal = false;
	}
	return retVal;
}
/*******************************************************************/
void CObjectExtractor::FlipToCOMOnTop(IplImage* input )
{
	IplImage* temp = cvCreateImage(cvSize(input->width,input->height),input->depth,1);
	cvThreshold(input,temp,static_cast<int>(m_threshold),255, CV_THRESH_BINARY_INV);
	double upper=0;
	double lower=0;
	CvRect rect;
	rect.width = input->width;
	rect.height = input->height/2;
	rect.x = 0;
	rect.y = input->height/2;
	cvSetImageROI(temp,rect);
	lower = GetLargestContourMass(temp);
	rect.y = 0;
	cvSetImageROI(temp,rect);
	upper = GetLargestContourMass(temp);
	// sorry for the magic number... for small values we ignore the flip
	if( lower > 1.10*upper )
	{ 
		cvFlip(input);
	}
	cvReleaseImage(&temp);
}
/*******************************************************************/
double CObjectExtractor::GetLargestContourMass( IplImage * img )
{
	CvSeq* contour = NULL;
	CvSeq* best = NULL;
	double length=0.00;
	double best_l = 0.00;
	double retVal = -1.00; 
	CvContourScanner scanner = cvStartFindContours( img,m_memStorage);//, sizeof(CvContour), CV_RETR_CCOMP);
	contour = cvFindNextContour( scanner );
	vector<CvSeq*> ctrs;
	
	if( NULL != contour )
	{
		while(contour)
		{
			length = cvContourPerimeter(contour);
			if( length > best_l )
			{
				best_l = length;
				best = contour;
			}
			contour = cvFindNextContour( scanner );
		}
		if( NULL != best )
		{
			retVal = cvContourArea(best);
		}
	}
	cvClearMemStorage(m_memStorage); 
	return retVal; 
}
/*******************************************************************/
void CObjectExtractor::SetMinWH( int minWH )
{
	m_minWH = minWH;
}
/*******************************************************************/
int CObjectExtractor::GetMinWH( void )
{
	return m_minWH; 
}
/*******************************************************************/
void CObjectExtractor::DoIntersectionTests(void)
{
	// go through the incomming bounding boxes and
	// merge any pair that overlap or are inside each other

}
/*******************************************************************/
bool CObjectExtractor::Intersect(CvRect a, CvRect b )
{
	bool retVal=false;
	return retVal;
}
/*******************************************************************/
CvRect CObjectExtractor::Merge(CvRect a, CvRect b)
{
	CvRect retVal;
	return retVal;
}
/*******************************************************************/